{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "import open3d as o3d\n",
    "import time\n",
    "\n",
    "# 读取深度图像\n",
    "depth_img = cv2.imread(\"../img/1_depth.png\", cv2.IMREAD_GRAYSCALE)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "w, h = depth_img.shape\n",
    "# 相机内参矩阵\n",
    "k = np.array([[535.4, 0.0, 320.0],\n",
    "              [0.0, 539.2, 240.0],\n",
    "              [0.0, 0.0, 1.0]])\n",
    "\n",
    "\n",
    "\n",
    "def depth_to_point_cloud_v2(depth):\n",
    "    # (w*h,3)，实际绝大多数时间用在这里了\n",
    "    static_array = np.array([np.array([i,j, 1]) for i in range(w) for j in range(h)])\n",
    "    \n",
    "    # 获取相机内参的逆矩阵\n",
    "    k_inv = np.linalg.inv(k)\n",
    "\n",
    "    # 每行乘逆矩阵\n",
    "    point_array = k_inv@static_array.T\n",
    "\n",
    "    depth_flat = depth.astype(np.float32)\n",
    "    depth_flat = depth_flat.reshape(-1,1)\n",
    "    # 每行乘深度值\n",
    "    point_cloud = depth_flat * point_array.T\n",
    "    return point_cloud\n",
    "\n",
    "pc = depth_to_point_cloud_v2(depth_img)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Time elapsed average 10 times:0.0044 seconds.\n"
     ]
    }
   ],
   "source": [
    "# 十次求平均\n",
    "elapsed_time_list = []\n",
    "for i in range(10):# \n",
    "    start_time = time.time()  # 记录开始时间\n",
    "\n",
    "    # 将深度图转换为点云\n",
    "    pc = depth_to_point_cloud_v2(depth_img)\n",
    "\n",
    "    end_time = time.time()  # 记录结束时间\n",
    "\n",
    "    elapsed_time = end_time - start_time  # 计算耗时\n",
    "\n",
    "    elapsed_time_list.append(elapsed_time)\n",
    "\n",
    "average_time = sum(elapsed_time_list) / len(elapsed_time_list)\n",
    "\n",
    "print(f\"Time elapsed average 10 times:{average_time:.4f} seconds.\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "         307226 function calls in 0.278 seconds\n",
      "\n",
      "   Ordered by: standard name\n",
      "\n",
      "   ncalls  tottime  percall  cumtime  percall filename:lineno(function)\n",
      "        1    0.068    0.068    0.193    0.193 2593389271.py:11(<listcomp>)\n",
      "        1    0.022    0.022    0.277    0.277 2593389271.py:9(depth_to_point_cloud_v2)\n",
      "        1    0.001    0.001    0.278    0.278 <string>:1(<module>)\n",
      "        1    0.000    0.000    0.000    0.000 linalg.py:130(get_linalg_error_extobj)\n",
      "        1    0.000    0.000    0.000    0.000 linalg.py:135(_makearray)\n",
      "        2    0.000    0.000    0.000    0.000 linalg.py:140(isComplexType)\n",
      "        1    0.000    0.000    0.000    0.000 linalg.py:153(_realType)\n",
      "        1    0.000    0.000    0.000    0.000 linalg.py:159(_commonType)\n",
      "        1    0.000    0.000    0.000    0.000 linalg.py:203(_assert_stacked_2d)\n",
      "        1    0.000    0.000    0.000    0.000 linalg.py:209(_assert_stacked_square)\n",
      "        1    0.000    0.000    0.000    0.000 linalg.py:488(_unary_dispatcher)\n",
      "        1    0.000    0.000    0.000    0.000 linalg.py:492(inv)\n",
      "        1    0.000    0.000    0.278    0.278 {built-in method builtins.exec}\n",
      "        1    0.000    0.000    0.000    0.000 {built-in method builtins.getattr}\n",
      "        3    0.000    0.000    0.000    0.000 {built-in method builtins.issubclass}\n",
      "   307201    0.186    0.000    0.186    0.000 {built-in method numpy.array}\n",
      "        1    0.000    0.000    0.000    0.000 {built-in method numpy.asarray}\n",
      "        1    0.000    0.000    0.000    0.000 {method '__array_prepare__' of 'numpy.ndarray' objects}\n",
      "        2    0.000    0.000    0.000    0.000 {method 'astype' of 'numpy.ndarray' objects}\n",
      "        1    0.000    0.000    0.000    0.000 {method 'disable' of '_lsprof.Profiler' objects}\n",
      "        1    0.000    0.000    0.000    0.000 {method 'get' of 'dict' objects}\n",
      "        1    0.000    0.000    0.000    0.000 {method 'reshape' of 'numpy.ndarray' objects}\n",
      "\n",
      "\n"
     ]
    }
   ],
   "source": [
    "import cProfile\n",
    "cProfile.run('depth_to_point_cloud_v2(depth_img)')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 可视化点云\n",
    "\n",
    "# 点云数据存储在名为point_cloud的numpy数组中\n",
    "cloud = o3d.geometry.PointCloud()\n",
    "cloud.points = o3d.utility.Vector3dVector(pc)\n",
    "\n",
    "# 创建一个可视化窗口\n",
    "vis = o3d.visualization.Visualizer()\n",
    "vis.create_window()\n",
    "\n",
    "# 将点云添加到可视化窗口中\n",
    "vis.add_geometry(cloud)\n",
    "\n",
    "# 设置相机位置，没调所以可有可无\n",
    "vis.get_view_control().set_front([0, 0, -1])\n",
    "vis.get_view_control().set_lookat([0, 0, 0])\n",
    "vis.get_view_control().set_up([0, -1, 0])\n",
    "\n",
    "# 开始显示点云\n",
    "vis.run()\n",
    "\n",
    "# 关闭显示窗口\n",
    "vis.destroy_window()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "0.2s"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "env4cv",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
